/**
 * 20220119：finalized by jordia
 */
import BroadPhase from "../broad-phase";
import CONSTANT from '../../constant';
import BvhTree from "./bvh-tree";
import GjkEpa from "../../collision-detector/gjk-epa-detector/gjk-epa";
import BvhProxy from "./bvh-proxy";
import BvhNode from './bvh-node';
import Method from "../../common/method";

/**
 * 层次包围树粗测
 */
export default class BvhBroadPhase extends BroadPhase{
    constructor() {
        super(CONSTANT.BROAD_PHASE_TYPE_BVH);
        this._incremental = true;
        this._tree = new BvhTree();
        this.movedProxies = new Array(1024);
        this.numMovedProxies = 0;
    }
    collide(n1,n2) {
        this._testCount++;
        let l1 = n1._height === 0;
        let l2 = n2._height === 0;
        if(n1 === n2) {
            if(l1) return;
            this.collide(n1._children[0],n2);
            this.collide(n1._children[1],n2);
            return;
        }
        let n1a=n1.size,n2a=n2.size;
        if(!Method.boxIntersectsBox(n1a,n2a)) return;
        if(l1 && l2) {
            let pp=this._setProxyPairList();
            pp._p1 = n1._proxy;
            pp._p2 = n2._proxy;
            return;
        }
        if(l2 || n1._height > n2._height) {
            this.collide(n1._children[0],n2);
            this.collide(n1._children[1],n2);
        } else {
            this.collide(n2._children[0],n1);
            this.collide(n2._children[1],n1);
        }
    }
    rayCastRecursive(node,x1,y1,z1,x2,y2,z2,callback) {
        let na=node.size;
        let tmp=this._raycastTest(na,x1,y1,z1,x2,y2,z2);
        if(!tmp) {
            return;
        }
        if(node._height === 0) {
            callback.process(node._proxy);
            return;
        }
        this.rayCastRecursive(node._children[0],x1,y1,z1,x2,y2,z2,callback);
        this.rayCastRecursive(node._children[1],x1,y1,z1,x2,y2,z2,callback);
    }
    convexCastRecursive(node,convex,begin,translation,callback) {
        let na=node.size,abi=this._aabb.min.elements,aba=this._aabb.max.elements;
        Method.copyElements(na,abi,0,0,3);
        Method.copyElements(na,aba,3,0,3);
        this._convexSweep.init(convex,begin,translation);
        let gjkEpa = GjkEpa.instance;
        if(!(gjkEpa.computeClosestPointsImpl(this._convexSweep,this._aabb,begin,this.identity,null,false) === 0 && gjkEpa.distance <= 0)) {
            return;
        }
        if(node._height === 0) {
            callback.process(node._proxy);
            return;
        }
        this.convexCastRecursive(node._children[0],convex,begin,translation,callback);
        this.convexCastRecursive(node._children[1],convex,begin,translation,callback);
    }
    aabbTestRecursive(node,aabb,callback) {
        let na=node.size,ae=aabb.elements;
        if(!Method.boxIntersectsBox(na,ae)) {
            return;
        }
        if(node._height === 0) {
            callback.process(node._proxy);
            return;
        }
        this.aabbTestRecursive(node._children[0],aabb,callback);
        this.aabbTestRecursive(node._children[1],aabb,callback);
    }
    _moveProxy(proxy,movedProxies,numMovedProxies){
        if(!proxy._moved) {
            proxy._moved = true;
            if(movedProxies.length === numMovedProxies) {
                let newArray = new Array(numMovedProxies << 1);
                let _g = 0;
                while(_g < numMovedProxies) {
                    let i = _g++;
                    newArray[i] = movedProxies[i];
                    movedProxies[i] = null;
                }
                movedProxies = newArray;
            }
            movedProxies[numMovedProxies] = proxy;
            return numMovedProxies+1;
        }
        return numMovedProxies;
    }
    moveProxy(proxy,aabb,displacement) {
        let p = proxy.size,ae=aabb.elements;
        if(Method.boxContainsBox(p,ae)) {
            return;
        }
        Method.copyElements(ae,p,0,0,6);
        let padding = CONSTANT.SETTING_BVH_PROXY_PADDING;
        Method.expandBoxByScale(p,padding);
        if(displacement != null) {
            let des=displacement.elements;
            Method.expandBoxByPoint(p,des[0],des[1],des[2]);
        }
        this.numMovedProxies=this._moveProxy(proxy,this.movedProxies,this.numMovedProxies);
    }
    _makeProxyChain(parent,child,ci){
        parent._children[ci] = child;
        child._parent = parent;
        child._childIndex = ci;
    }
    _setParentProxy(parent){
        Method.boxUnionBox(parent._children[0].size,parent._children[1].size,parent.size);
        let h1 = parent._children[0]._height, h2 = parent._children[1]._height;
        parent._height = (h1 > h2 ? h1 : h2) + 1;
    }
    setProxyChain(parent,self,si,child,ci){
        this._makeProxyChain(parent,self,si);
        this._makeProxyChain(self,child,ci);
        this._setParentProxy(parent);
        this._setParentProxy(self);
    }
    updateNode(nd,tree){
        let node=nd;
        while(node != null) {
            if(tree._strategy._balancingEnabled) {
                if(node._height >= 2) {
                    let p = node._parent, l = node._children[0], r = node._children[1];
                    let balance = l._height - r._height;
                    let nodeIndex = node._childIndex;
                    if(balance > 1) {
                        let ll = l._children[0], lr = l._children[1];
                        if(ll._height > lr._height) {
                            this.setProxyChain(l,node,1,lr,0);
                        } else {
                            this.setProxyChain(l,node,0,ll,0);
                        }
                        if(p != null) {
                            this._makeProxyChain(p,l,nodeIndex);
                        } else {
                            tree._root = l;
                            l._parent = null;
                        }
                        node = l;
                    } else if(balance < -1) {
                        let rl = r._children[0], rr = r._children[1];
                        if(rl._height > rr._height) {
                            this.setProxyChain(r,node,1,rr,1);
                        } else {
                            this.setProxyChain(r,node,0,rl,1);
                        }
                        if(p != null) {
                            this._makeProxyChain(p,r,nodeIndex);
                        } else {
                            tree._root = r;
                            r._parent = null;
                        }
                        node = r;
                    }
                }
            }
            this._setParentProxy(node);
            node = node._parent;
        }
    }
    _addLeaf(tree,leaf){
        tree._numLeaves++;
        if(tree.leafList == null) {
            tree.leafList = leaf;
            tree.leafListLast = leaf;
        } else {
            tree.leafListLast._nextLeaf = leaf;
            leaf._prevLeaf = tree.leafListLast;
            tree.leafListLast = leaf;
        }
    }
    createProxy(userData,aabb) {
        let p = new BvhProxy(userData,this._idCount++);
        this._numProxies=this._setProxyList(this._numProxies,p,aabb);
        let padding = CONSTANT.SETTING_BVH_PROXY_PADDING;
        Method.expandBoxByScale(p.size,padding);
        let _this = this._tree;
        let first = _this._nodePool;
        if(first != null) {
            _this._nodePool = first._next;
            first._next = null;
        } else {
            first = new BvhNode();
        }
        let leaf = first;
        leaf._proxy = p;
        p._leaf = leaf;
        Method.copyElements(p.size,leaf.size,0,0,6);
        this._addLeaf(_this,leaf);
        if(_this._root == null) {
            _this._root = leaf;
        } else {
            let sibling = _this._root;
            while(sibling._height > 0) {
                let nextStep = _this._strategy._decideInsertion(sibling,leaf);
                if(nextStep === -1) {
                    break;
                } else {
                    sibling = sibling._children[nextStep];
                }
            }
            let parent = sibling._parent;
            let first = _this._nodePool;
            if(first != null) {
                _this._nodePool = first._next;
                first._next = null;
            } else {
                first = new BvhNode();
            }
            let node = first;
            if(parent == null) {
                _this._root = node;
            } else {
                this._makeProxyChain(parent,node,sibling._childIndex);
            }
            this._makeProxyChain(node,sibling,sibling._childIndex);
            this._makeProxyChain(node,leaf,sibling._childIndex ^ 1);
            this.updateNode(node,_this);
        }
        this.numMovedProxies=this._moveProxy(p,this.movedProxies,this.numMovedProxies);
        return p;
    }
    _resetChild(parent,tree){
        parent._next = null;
        parent._children[0] = parent._children[1] = null;
        parent._childIndex = 0;
        parent._parent = null;
        parent._height = 0;
        parent._proxy = null;
        parent._next = tree._nodePool;
        tree._nodePool = parent;
    }
    _resetBvhProxy(bvhProxy){
        let _this = this._tree;
        let leaf = bvhProxy._leaf;
        _this._numLeaves--;
        let prev1 = leaf._prevLeaf, next1 = leaf._nextLeaf;
        if(prev1 != null) prev1._nextLeaf = next1;
        if(next1 != null) next1._prevLeaf = prev1;
        if(leaf === _this.leafList) _this.leafList = _this.leafList._nextLeaf;
        if(leaf === _this.leafListLast) _this.leafListLast = _this.leafListLast._prevLeaf;
        leaf._nextLeaf = leaf._prevLeaf = null;
        if(_this._root === leaf) {
            _this._root = null;
        } else {
            let parent = leaf._parent;
            let sibling = parent._children[leaf._childIndex ^ 1];
            let grandParent = parent._parent;
            if(grandParent == null) {
                sibling._parent = null;
                sibling._childIndex = 0;
                _this._root = sibling;
                this._resetChild(parent,_this);
            } else {
                sibling._parent = grandParent;
                let index = parent._childIndex;
                grandParent._children[index] = sibling;
                sibling._parent = grandParent;
                sibling._childIndex = index;
                this._resetChild(parent,_this);
                this.updateNode(grandParent,_this);
            }
        }
        bvhProxy._leaf = null;
        this._resetChild(leaf,_this);
    }
    destroyProxy(proxy) {
        this._numProxies--;
        let prev = proxy._prev, next = proxy._next;
        if(prev != null) prev._next = next;
        if(next != null) next._prev = prev;
        if(proxy === this._proxyList) this._proxyList = this._proxyList._next;
        if(proxy === this._proxyListLast) this._proxyListLast = this._proxyListLast._prev;
        proxy._next = proxy._prev = null;
        let bvhProxy = proxy;
        this._resetBvhProxy(bvhProxy);
        bvhProxy.userData = null;
        bvhProxy._next = null;
        bvhProxy._prev = null;
        if(bvhProxy._moved) {
            bvhProxy._moved = false;
        }
    }
    collectPairs() {
        let p = this._proxyPairList;
        if(p != null) {
            while(true) {
                p._p1 = p._p2 = null;
                p = p._next;
                if(!(p != null)) break;
            }
            this._proxyPairList._next = this._proxyPairPool;
            this._proxyPairPool = this._proxyPairList;
            this._proxyPairList = null;
        }
        this._testCount = 0;
        if(this._numProxies < 2) return;
        let incrementalCollision = this.numMovedProxies / this._numProxies < CONSTANT.SETTING_BVH_INCREMENTAL_COLLISION_THRESHOLD;
        let _g = 0, _g1 = this.numMovedProxies;
        while(_g < _g1) {
            let i = _g++;
            let p = this.movedProxies[i];
            if(p._moved) {
                this._resetBvhProxy(p);
                let _this1 = this._tree;
                let first = _this1._nodePool;
                if(first != null) {
                    _this1._nodePool = first._next;
                    first._next = null;
                } else {
                    first = new BvhNode();
                }
                let leaf1 = first;
                leaf1._proxy = p;
                p._leaf = leaf1;
                Method.copyElements(p.size,leaf1.size,0,0,6);
                this._addLeaf(_this1,leaf1);
                if(_this1._root == null) {
                    _this1._root = leaf1;
                } else {
                    let sibling = _this1._root;
                    while(sibling._height > 0) {
                        let nextStep = _this1._strategy._decideInsertion(sibling,leaf1);
                        if(nextStep === -1) {
                            break;
                        } else {
                            sibling = sibling._children[nextStep];
                        }
                    }
                    let parent = sibling._parent;
                    let first = _this1._nodePool;
                    if(first != null) {
                        _this1._nodePool = first._next;
                        first._next = null;
                    } else {
                        first = new BvhNode();
                    }
                    let node = first;
                    if(parent == null) {
                        _this1._root = node;
                    } else {
                        this._makeProxyChain(parent,node,sibling._childIndex);
                    }
                    this._makeProxyChain(node,sibling,sibling._childIndex);     //0 1 2 3 4 5 6 7...
                    this._makeProxyChain(node,leaf1,sibling._childIndex^1); //1 0 3 2 5 4 7 6...
                    this.updateNode(node,_this1);
                }
                if(incrementalCollision) {
                    this.collide(this._tree._root,p._leaf);
                }
                p._moved = false;
            }
            this.movedProxies[i] = null;
        }
        if(!incrementalCollision) {
            this.collide(this._tree._root,this._tree._root);
        }
        this.numMovedProxies = 0;
    }
    rayCast(begin,end,callback) {
        if(this._tree._root == null) return;
        let p1e=begin.elements;
        let p2e=end.elements;
        this.rayCastRecursive(this._tree._root,p1e[0],p1e[1],p1e[2],p2e[0],p2e[1],p2e[2],callback);
    }
    convexCast(convex,begin,translation,callback) {
        if(this._tree._root == null) return;
        this.convexCastRecursive(this._tree._root,convex,begin,translation,callback);
    }
    aabbTest(aabb,callback) {
        if(this._tree._root == null) return;
        this.aabbTestRecursive(this._tree._root,aabb,callback);
    }
    getTreeBalance() {
        return this._tree._getBalance();
    }
}